#include "PID_controller.h"

P rtP = {

  7.0,

  0.35,

  0.7,

  0.05,

  0.15,

  3.5,

  0.9,

  0.0,

  7.0,

  0.35,

  0.7,

  0.05,

  0.15,

  3.5,

  2.8,

  0.35,

  0.5,

  0.1,

  0.3,

  3.5,

  1.0,

  1.0,

  1.0,

  0.002,

  0.0,

  0.002,

  0.0,

  0.002,

  0.0
};

DW rtDW;
RT_MODEL rtM_;
RT_MODEL *const rtM = &rtM_;
static void eulerdot2pqr(const real_T rtu_eulerdot[3], const real_T rtu_euler[3],
  real_T *rty_pqr, real_T *rty_pqr_j, real_T *rty_pqr_g);
static void motor_signal_distribution(real_T rtu_throttle_d, real_T
  rtu_yaw_distribution, real_T rtu_roll_distribution, real_T
  rtu_pitch_distribution, real_T *rty_pwm1, real_T *rty_pwm2, real_T *rty_pwm3,
  real_T *rty_pwm4);
static void rate_scheduler(void);
static void rate_scheduler(void)
{
  (rtM->Timing.TaskCounters.TID[1])++;
  if ((rtM->Timing.TaskCounters.TID[1]) > 4) {
    rtM->Timing.TaskCounters.TID[1] = 0;
  }
}

static void eulerdot2pqr(const real_T rtu_eulerdot[3], const real_T rtu_euler[3],
  real_T *rty_pqr, real_T *rty_pqr_j, real_T *rty_pqr_g)
{
  real_T tmp;
  real_T tmp_0;
  real_T tmp_1;
  *rty_pqr = rtu_eulerdot[0] - rtu_eulerdot[2] * sin(rtu_euler[1]);
  tmp = rtu_eulerdot[2] * cos(rtu_euler[1]);
  tmp_0 = cos(rtu_euler[0]);
  tmp_1 = sin(rtu_euler[0]);
  *rty_pqr_j = tmp * tmp_1 + rtu_eulerdot[1] * tmp_0;
  *rty_pqr_g = tmp * tmp_0 + -rtu_eulerdot[1] * tmp_1;
}

static void motor_signal_distribution(real_T rtu_throttle_d, real_T
  rtu_yaw_distribution, real_T rtu_roll_distribution, real_T
  rtu_pitch_distribution, real_T *rty_pwm1, real_T *rty_pwm2, real_T *rty_pwm3,
  real_T *rty_pwm4)
{
  real_T u0;
  real_T u0_tmp;
  u0_tmp = rtu_throttle_d + rtu_yaw_distribution;
  u0 = (u0_tmp - rtu_roll_distribution) + rtu_pitch_distribution;
  if (u0 > rtP.PWM_MAX) {
    *rty_pwm1 = rtP.PWM_MAX;
  } else if (u0 < rtP.PWM_MIN) {
    *rty_pwm1 = rtP.PWM_MIN;
  } else {
    *rty_pwm1 = u0;
  }

  u0 = (u0_tmp + rtu_roll_distribution) - rtu_pitch_distribution;
  if (u0 > rtP.PWM_MAX) {
    *rty_pwm2 = rtP.PWM_MAX;
  } else if (u0 < rtP.PWM_MIN) {
    *rty_pwm2 = rtP.PWM_MIN;
  } else {
    *rty_pwm2 = u0;
  }

  u0_tmp = rtu_throttle_d - rtu_yaw_distribution;
  u0 = (u0_tmp + rtu_roll_distribution) + rtu_pitch_distribution;
  if (u0 > rtP.PWM_MAX) {
    *rty_pwm3 = rtP.PWM_MAX;
  } else if (u0 < rtP.PWM_MIN) {
    *rty_pwm3 = rtP.PWM_MIN;
  } else {
    *rty_pwm3 = u0;
  }

  u0 = (u0_tmp - rtu_roll_distribution) - rtu_pitch_distribution;
  if (u0 > rtP.PWM_MAX) {
    *rty_pwm4 = rtP.PWM_MAX;
  } else if (u0 < rtP.PWM_MIN) {
    *rty_pwm4 = rtP.PWM_MIN;
  } else {
    *rty_pwm4 = u0;
  }
}

void PID_step(angel_desired *arg_angle_desired, angle_rate *arg_angle_rate,
              angle_solved *arg_angle_solved, pwm_controller_out *arg_pwm)
{
  real_T rtb_TmpSignalConversionAteulerd[3];
  real_T rtb_TmpSignalConversionAteule_e[3];
  real_T rtb_Integrator_b;
  real_T rtb_Sum;
  real_T rtb_Sum_b;
  if (rtM->Timing.TaskCounters.TID[1] == 0) {
    rtb_TmpSignalConversionAteulerd[0] = arg_angle_solved->roll_s;
    rtb_TmpSignalConversionAteulerd[1] = arg_angle_solved->pitch_s;
    rtb_TmpSignalConversionAteulerd[2] = arg_angle_solved->yaw_s;
    if (arg_angle_desired->roll_d > rtP.ROLL_MAX) {
      rtb_Integrator_b = rtP.ROLL_MAX;
    } else if (arg_angle_desired->roll_d < -rtP.ROLL_MAX) {
      rtb_Integrator_b = -rtP.ROLL_MAX;
    } else {
      rtb_Integrator_b = arg_angle_desired->roll_d;
    }

    rtDW.roll_e = rtb_Integrator_b - arg_angle_solved->roll_s;
    rtDW.roll_rate_d = rtP.ROLL_P * rtDW.roll_e;
    if (rtDW.roll_rate_d > rtP.ROLL_RATE_MAX) {
      rtDW.roll_rate_d = rtP.ROLL_RATE_MAX;
    } else {
      if (rtDW.roll_rate_d < -rtP.ROLL_RATE_MAX) {
        rtDW.roll_rate_d = -rtP.ROLL_RATE_MAX;
      }
    }

    if (arg_angle_desired->pitch_d > rtP.PITCH_MAX) {
      rtb_Integrator_b = rtP.PITCH_MAX;
    } else if (arg_angle_desired->pitch_d < -rtP.PITCH_MAX) {
      rtb_Integrator_b = -rtP.PITCH_MAX;
    } else {
      rtb_Integrator_b = arg_angle_desired->pitch_d;
    }

    rtDW.pitch_e = rtb_Integrator_b - arg_angle_solved->pitch_s;
    rtDW.pitch_rate_d = rtP.PITCH_P * rtDW.pitch_e;
    if (rtDW.pitch_rate_d > rtP.PITCH_RATE_MAX) {
      rtDW.pitch_rate_d = rtP.PITCH_RATE_MAX;
    } else {
      if (rtDW.pitch_rate_d < -rtP.PITCH_RATE_MAX) {
        rtDW.pitch_rate_d = -rtP.PITCH_RATE_MAX;
      }
    }

    rtDW.yaw_e = arg_angle_desired->yaw_d - arg_angle_solved->yaw_s;
    rtDW.yaw_rate_d = rtP.YAW_P * rtDW.yaw_e;
    if (rtDW.yaw_rate_d > rtP.YAW_RATE_MAX) {
      rtDW.yaw_rate_d = rtP.YAW_RATE_MAX;
    } else {
      if (rtDW.yaw_rate_d < -rtP.YAW_RATE_MAX) {
        rtDW.yaw_rate_d = -rtP.YAW_RATE_MAX;
      }
    }

    rtb_TmpSignalConversionAteule_e[0] = rtDW.roll_rate_d;
    rtb_TmpSignalConversionAteule_e[1] = rtDW.pitch_rate_d;
    rtb_TmpSignalConversionAteule_e[2] = rtDW.yaw_rate_d;
    eulerdot2pqr(rtb_TmpSignalConversionAteule_e,
                 rtb_TmpSignalConversionAteulerd, &rtDW.w1, &rtDW.w2, &rtDW.w3);
    rtDW.yaw_feedforward_gain = rtP.YAW_FEEDFORWARD_GAIN *
      arg_angle_desired->yaw_d;
  }

  rtDW.r_e = (rtDW.yaw_feedforward_gain + rtDW.w3) - arg_angle_rate->r_m;
  rtb_Integrator_b = rtP.YAW_RATE_P * rtDW.r_e + rtDW.yaw_rate_e_int;
  if (rtb_Integrator_b > rtP.YAW_DISTRIBUTION_MAX) {
    rtDW.yaw_distribution = rtP.YAW_DISTRIBUTION_MAX;
  } else if (rtb_Integrator_b < -rtP.YAW_DISTRIBUTION_MAX) {
    rtDW.yaw_distribution = -rtP.YAW_DISTRIBUTION_MAX;
  } else {
    rtDW.yaw_distribution = rtb_Integrator_b;
  }

  rtDW.p_e = rtDW.w1 - arg_angle_rate->p_m;
  rtb_Sum = rtP.ROLL_RATE_P * rtDW.p_e + rtDW.roll_rate_e_int;
  if (rtb_Sum > rtP.ROLL_DISTRIBUTION_MAX) {
    rtDW.roll_distribution = rtP.ROLL_DISTRIBUTION_MAX;
  } else if (rtb_Sum < -rtP.ROLL_DISTRIBUTION_MAX) {
    rtDW.roll_distribution = -rtP.ROLL_DISTRIBUTION_MAX;
  } else {
    rtDW.roll_distribution = rtb_Sum;
  }

  rtDW.q_e = rtDW.w2 - arg_angle_rate->q_m;
  rtb_Sum_b = rtP.PITCH_RATE_P * rtDW.q_e + rtDW.pitch_rate_e_int;
  if (rtb_Sum_b > rtP.PITCH_DISTRIBUTION_MAX) {
    rtDW.pitch_distribution = rtP.PITCH_DISTRIBUTION_MAX;
  } else if (rtb_Sum_b < -rtP.PITCH_DISTRIBUTION_MAX) {
    rtDW.pitch_distribution = -rtP.PITCH_DISTRIBUTION_MAX;
  } else {
    rtDW.pitch_distribution = rtb_Sum_b;
  }

  motor_signal_distribution(arg_angle_desired->throttle_d, rtDW.yaw_distribution,
    rtDW.roll_distribution, rtDW.pitch_distribution, &arg_pwm->pwm1,
    &arg_pwm->pwm2, &arg_pwm->pwm3, &arg_pwm->pwm4);
  rtDW.yaw_rate_e_int += ((rtDW.yaw_distribution - rtb_Integrator_b) *
    rtP.yaw_rate_PID_Kb + rtP.YAW_RATE_I * rtDW.r_e) * rtP.Integrator_gainval;
  rtDW.roll_rate_e_int += ((rtDW.roll_distribution - rtb_Sum) *
    rtP.roll_rate_PID_Kb + rtP.ROLL_RATE_I * rtDW.p_e) *
    rtP.Integrator_gainval_p;
  rtDW.pitch_rate_e_int += ((rtDW.pitch_distribution - rtb_Sum_b) *
    rtP.pitch_rate_PID_Kb + rtP.PITCH_RATE_I * rtDW.q_e) *
    rtP.Integrator_gainval_j;
  rate_scheduler();
}

void PID_init(void)
{
  rtDW.yaw_rate_e_int = rtP.Integrator_IC;
  rtDW.roll_rate_e_int = rtP.Integrator_IC_p;
  rtDW.pitch_rate_e_int = rtP.Integrator_IC_l;
}
